import matplotlib.pyplot as plt
import math


def ict1(x, y, rad):
    #[x1, y1]=[x, y]+[math.cos(rad), math.sin(rad)]*l
    #(xaxis-x)/(0-y)=math.cos(rad)/math.sin(rad)
    xaxis=x-y/math.tan(rad)
    #(yaxis-y)/(0-x)=math.sin(rad)/math.cos(rad)
    ly=-x/math.cos(rad)
    lx=-y/math.sin(rad)
    if lx<ly:
        return [x+lx*math.cos(rad), 0]
    else:
        return [0, y+ly*math.sin(rad)]
        
def fix_rad(rad):
    while rad>math.pi:
        rad=rad-2*math.pi
    while rad<-math.pi:
        rad=rad+2*math.pi
    return rad
        
        
def reflectx(rad):
    return -rad
    
         
def reflecty(rad):
    if rad<0:
        return -math.pi-rad            
    else:
        return math.pi-rad
        
def ray2(x, y, rad, l2=130):
    rad=fix_rad(rad)
    [x1, y1]=ict1(x, y, rad)
    x2=0
    y2=0
    rad1=0
    rad2=0
    if x1==0:
        rad1=reflecty(rad)
        lx=-y1/math.sin(rad1)
        y2=0
        x2=x1+lx*math.cos(rad1)
        rad2=reflectx(rad1)
    else:
        rad1=reflectx(rad)
        ly=-x1/math.cos(rad1)
        x2=0
        y2=y1+ly*math.sin(rad1)
        rad2=reflecty(rad1)       
    
    x3=x2+l2*math.cos(rad2)
    y3=y2+l2*math.sin(rad2)
    
    return [[x, x1, x2, x3], [y, y1, y2, y3]]
    

def plot_ray(x, y, deg, title, l2=130):       
    ray0=ray2(x, y, math.radians(deg+hfov), l2)
    ray1=ray2(x, y, math.radians(deg-hfov), l2)
    [oax, oay]=ict1(x, y, math.radians(deg))
    [oax1, oay1]=ict1(x, y, math.radians(deg+90))
    [oax2, oay2]=ict1(x, y, math.radians(deg-90))
    plt.plot(ray0[0], ray0[1], 'g')
    plt.plot(ray1[0], ray1[1], 'r')
    plt.plot([x, oax], [y, oay], 'b')
    plt.plot([oax1, oax2], [oay1, oay2], 'b')
    plt.axis('equal')
    plt.grid(True)    
    plt.title(title)
    plt.show()


def f1(rad, hfovrad):
    rad2=-math.pi-(rad-hfovrad)
    rad1=rad+math.pi/2
    return 2*math.sin(rad2)/math.sin(rad1-rad2)


def f2(rad):
	return -1/math.cos(rad-math.pi/2)
	
def f1_to_f2(in_d, out_d, thick, hfovrad):
	return (out_d+thick*math.tan(hfovrad))/in_d
	
def solve_cos_2rad_minus_hfov(in_d, out_d, thick, hfovrad):
	return in_d*math.cos(hfovrad)/(in_d+out_d+thick*math.tan(hfovrad))
	
	
def solve_rad2(in_d, out_d, thick, hfovrad):
	costheta=solve_cos_2rad_minus_hfov(in_d, out_d, thick, hfovrad)
	theta1=(hfovrad+math.acos(costheta))/2
	theta2=(hfovrad-math.acos(costheta))/2
	#print(theta1)
	#print(theta2)
	rad=[]
	if theta1>0:
		if theta1<math.pi/2:
			rad.append(theta1-math.pi)
	if theta2>0:
		if theta2<math.pi/2:
			rad.append(theta2-math.pi)
	return rad
	
	
	
def solve_tan_2rad(in_d, out_d, thick, hfovrad):
	return 1/(in_d/(out_d*math.cos(hfovrad)+thick*math.sin(hfovrad))-math.tan(hfovrad))	
    
def solve_rad(in_d, out_d, thick, hfovrad):
	radd=math.atan(solve_tan_2rad(in_d, out_d, thick, hfovrad))
	if radd>0:
		return radd/2-math.pi
	else:
		return (radd-math.pi)/2  
		
def solve_deg_x_y(in_d, out_d, thick, hfovrad):
	rad=solve_rad2(in_d, out_d, thick, hfovrad)
	#if isempty(rad):
		#return []
		
	x=in_d/f2(rad[0])
	#print(x)
	
	k1=out_d+thick*math.tan(hfovrad)
	#print(k1/f1(rad[0], hfovrad))
	rad1=rad[0]+math.pi/2
	rad3=rad[0]+hfovrad+math.pi
	#print(rad3)
	k3=(2*x+k1*math.cos(rad1))/math.cos(rad3)
	y=(k3*math.sin(rad3)-k1*math.sin(rad1))/2
	return [math.degrees(rad[0]), x, y]
	
	
def to_grid_spec(deg, x, y, hfov):
	yrad=math.radians(deg-90)
	ye=y-x/math.cos(yrad)*math.sin(yrad)
	xrad=math.radians(-180-(deg-hfov))
	xe=-x-y/math.sin(xrad)*math.cos(xrad)
	return [xe, ye, 180+deg]

   
def plot_f1(hfov):
    x=range(-180+hfov, -90-hfov)  
    #print(x) 
    y=[]
    y1=[]
    for x1 in x:
        y.append(f1(math.radians(x1), math.radians(hfov)))
        y1.append(f2(math.radians(x1)))
    plt.plot(x, y, 'b')
    plt.plot(x, y1, 'g')
    #plt.axis('equal')
    plt.grid(True)
    title='hfov='+str(hfov)
    plt.title(title)
    plt.show()
            

hfov=30
in_d=5
out_d=8
thick=8
ratio=3.0/4.0

[deg, x, y]=solve_deg_x_y(in_d, out_d, thick, math.radians(hfov))

[xg, yg, degg]=to_grid_spec(deg, x, y, hfov)

print([xg, yg, degg])

title=str([hfov, ("%.2f" %xg), ("%.2f" %yg), ("%.2f" %(xg*ratio)), ("%.2f" %degg), in_d, out_d, thick])
plot_ray(x, y, deg, title, 150)
#plot_f1(30)


